import cv2
import numpy as np
import glob


'''
作者：SXL；
日期：2022.9.15；
功能：利用相机内参和5个畸变参数对图像进行去畸变处理；
输入：左相机内参mtx_l和5个畸变系数distCoeffL，右相机内参mtx_r和5个畸变系数distCoeffR;
输出：去畸变的左图像dstl和右图像dstr，新的左相机内参newcameramtx_l和右相机内参newcameramtx_r;
'''
mtx_l = np.array([[1573.58151906911,0,662.895431234695,0],
                  [0,1573.59522335107,537.204064426375,0],
                  [0, 0, 1,0]])

distCoeffL = np.array([-0.0831505168909851 ,0.0656100655702405 ,0.000920729968572471,0.000152668039388092,0.447765830386160])
cameraMatrixL=mtx_l[:,0:3]

mtx_r =  np.array([[1573.71414602399, 0,617.570635183454,0],
                   [0,1573.67713462260,482.001764645626,0],
                   [0, 0, 1,0]])

distCoeffR = np.array([-0.0933141628884144, 0.128908619365820,-0.000108196062470541,0.000320193782901305,0.294326104325591])
cameraMatrixR=mtx_r[:,0:3]

# 图片路径
images = glob.glob('E:/0915/stereo/ALL/*.bmp')
save_path = 'LR/'
save_path_l = save_path+'L/'
save_path_r = save_path+'R/'
for fnamel in images:
    imgl = cv2.imread(fnamel)
    fnamer = fnamel.replace('ALL','ARR')
    fnamer = fnamer.replace('L','R')
    imgr = cv2.imread(fnamer)

    h,  w = imgl.shape[:2]
    # 去畸变后的左内参
    newcameramtx_l, roi_l = cv2.getOptimalNewCameraMatrix(cameraMatrixL, distCoeffL, (w, h), 1, (w, h))
    # 去畸变后的左图片
    dstl = cv2.undistort(imgl, cameraMatrixL, distCoeffL, None, newcameramtx_l)
    x, y, w, h = roi_l
    dstl = dstl[y:y+h, x:x+w]
    save_namel = save_path_l+fnamel.split('\\')[-1]
    cv2.imwrite(save_namel,dstl)


    h,  w = imgr.shape[:2]
    # 去畸变后的右内参
    newcameramtx_r, roi_r = cv2.getOptimalNewCameraMatrix(cameraMatrixR, distCoeffR, (w, h), 1, (w, h))
    # 去畸变后的右图片
    dstr = cv2.undistort(imgr, cameraMatrixR, distCoeffR, None, newcameramtx_r)
    x, y, w, h = roi_r
    dstr = dstr[y:y+h, x:x+w]
    save_namer = save_path_r+fnamer.split('\\')[-1]
    cv2.imwrite(save_namer,dstr)


